#include "algo_imu.h"

// 变量初始化
butterworth_buffer_t 		g_butterworth_buffer1_acc[3];	// 加速度第一次滤波缓冲
butterworth_buffer_t 		g_butterworth_buffer2_acc[3];	// 加速度第二次滤波缓冲
butterworth_buffer_t 		g_butterworth_buffer1_gyro[3];	// 角速度第一次滤波缓冲
butterworth_buffer_t 		g_butterworth_buffer2_gyro[3];	// 角速度第二次滤波缓冲
butterworth_parameter_t 	g_butterworth_parameter_acc;	// 加速度滤波器参数
butterworth_parameter_t 	g_butterworth_parameter_gyro; 	// 角速度滤波器参数

// IMU算法初始化
// sampling_period：采样周期
// acc_：陀螺仪原始加速度数据指针
// gyro_：陀螺仪原始角速度数据指针
void imu_init(algo_imu_t *imu, f32 sampling_period, int16 *acc_x, int16 *acc_y, int16 *acc_z, int16 *gyro_x, int16 *gyro_y, int16 *gyro_z)
{
	imu->sampling_period = sampling_period;
	
	imu->acc_raw_ptr.x = acc_x;
	imu->acc_raw_ptr.y = acc_y;
	imu->acc_raw_ptr.z = acc_z;
	imu->gyro_raw_ptr.x = gyro_x;
	imu->gyro_raw_ptr.y = gyro_y;
	imu->gyro_raw_ptr.z = gyro_z;
	
	set_cutoff_frequency(&g_butterworth_parameter_acc, 200, 30); // 设置加速度滤波器参数
	set_cutoff_frequency(&g_butterworth_parameter_gyro, 200, 50);// 设置角速度滤波器参数
}

// IMU数据处理
void imu_data_proc(algo_imu_t *imu)
{
	int16 filter1_acc_x, filter1_acc_y, filter1_acc_z;
	int16 filter2_acc_x, filter2_acc_y, filter2_acc_z;
	int16 filter1_gyro_x, filter1_gyro_y, filter1_gyro_z;
	int16 filter2_gyro_x, filter2_gyro_y, filter2_gyro_z;
	
	f32 pitch_acc_angle = 0, pitch_gyro_angle = 0;
	f32 roll_acc_angle = 0, roll_gyro_angle = 0;
	
	// 第一次低通滤波
	filter1_acc_x = butterworth_lowpass_filter(&g_bandstop_filter_parameter_30_94, &g_butterworth_buffer1_acc[0], *imu->acc_raw_ptr.x);
	filter1_acc_y = butterworth_lowpass_filter(&g_bandstop_filter_parameter_30_94, &g_butterworth_buffer1_acc[1], *imu->acc_raw_ptr.y);
	filter1_acc_z = butterworth_lowpass_filter(&g_bandstop_filter_parameter_30_94, &g_butterworth_buffer1_acc[2], *imu->acc_raw_ptr.z);
	
	filter1_gyro_x = butterworth_lowpass_filter(&g_bandstop_filter_parameter_30_98, &g_butterworth_buffer1_gyro[0], *imu->gyro_raw_ptr.x);
	filter1_gyro_y = butterworth_lowpass_filter(&g_bandstop_filter_parameter_30_98, &g_butterworth_buffer1_gyro[1], *imu->gyro_raw_ptr.y);
	filter1_gyro_z = butterworth_lowpass_filter(&g_bandstop_filter_parameter_30_98, &g_butterworth_buffer1_gyro[2], *imu->gyro_raw_ptr.z);
	
	// 第二次低通滤波
	filter2_acc_x = butterworth_lowpass_filter(&g_butterworth_parameter_acc, &g_butterworth_buffer2_acc[0], filter1_acc_x);
	filter2_acc_y = butterworth_lowpass_filter(&g_butterworth_parameter_acc, &g_butterworth_buffer2_acc[1], filter1_acc_y);
	filter2_acc_z = butterworth_lowpass_filter(&g_butterworth_parameter_acc, &g_butterworth_buffer2_acc[2], filter1_acc_z);
	
	filter2_gyro_x = butterworth_lowpass_filter(&g_butterworth_parameter_gyro, &g_butterworth_buffer2_gyro[0], filter1_gyro_x);
	filter2_gyro_y = butterworth_lowpass_filter(&g_butterworth_parameter_gyro, &g_butterworth_buffer2_gyro[1], filter1_gyro_y);
	filter2_gyro_z = butterworth_lowpass_filter(&g_butterworth_parameter_gyro, &g_butterworth_buffer2_gyro[2], filter1_gyro_z);
	
	// 单位转换
	imu->acc.x = (f32)filter2_acc_x / ACC_SENSITIVITY;
	imu->acc.y = (f32)filter2_acc_y / ACC_SENSITIVITY;
	imu->acc.z = (f32)filter2_acc_z / ACC_SENSITIVITY;
	
	imu->gyro.x = (f32)filter2_gyro_x / GYRO_SENSITIVITY;
	imu->gyro.y = (f32)filter2_gyro_y / GYRO_SENSITIVITY;
	imu->gyro.z = (f32)filter2_gyro_z / GYRO_SENSITIVITY;
	
	// 数值补偿
	imu->gyro.x -= 8.11f;
	imu->gyro.y += 2.12f;
	imu->gyro.y = -imu->gyro.y;
	
	// 计算欧拉角，互补滤波（各姿态角方向根据实际陀螺仪安装调整）
	pitch_gyro_angle = imu->pitch.angle + imu->gyro.y * imu->sampling_period;
	roll_gyro_angle = imu->roll.angle + imu->gyro.x * imu->sampling_period;
	pitch_acc_angle = -57.3f * atan(imu->acc.x * inv_sqrt(imu->acc.y * imu->acc.y + imu->acc.z * imu->acc.z));
	roll_acc_angle = -57.3f * atan(imu->acc.y * inv_sqrt(imu->acc.x * imu->acc.x + imu->acc.z * imu->acc.z));
	// 互补滤波需要保证角速度与加速度计算出的角度值方向相同！
	imu->pitch.angle = 0.98f * pitch_gyro_angle + 0.02f * pitch_acc_angle;		// 俯仰角度
	imu->roll.angle = 0.98f * roll_gyro_angle + 0.02f * roll_acc_angle;			// 横滚角度
	imu->yaw.angle = imu->yaw.angle + imu->gyro.z * imu->sampling_period;		// 偏航角度
	
	imu->pitch.gyro = imu->gyro.y;	// 俯仰角速度
	imu->roll.gyro = imu->gyro.x;	// 横滚角速度
	imu->yaw.gyro = imu->gyro.z;	// 偏航角速度
}

